#ifndef VISION_CONTROLLERBW_H_
#define VISION_CONTROLLERBW_H_

#include <opencv2/opencv.hpp>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <vector>
#include <limits>
#include <string>
#include "Marker.h"
#include "Target.h"
#include "BWPatternFiltering.h"

using namespace std;
using namespace cv;

class VisionControllerBW {
private:

	//Stati in cui pu�� trovarsi l'agente
	enum VisionState {
		ADAPTIVE_MARKER, ADAPTIVE_GRID, BACKLIGHT
	};

	//Stato della ricerca del marker
	VisionState state;

	//Gestore della webcam
	VideoCapture* camera;
	//Mutex necessario alla gestione della concorrenza nel metodo processFrame
	pthread_mutex_t mutex;
	Mat frame, depthMap;
	//massimo numero di frame ghost da considerare quando si perde il target
	int maxGhostFrames;
	//ultimo target riconosciuto
	Target* lastTarget;
	bool lastTargetIsValid;
	double lastMeanValue;
	//tolleranza di profondit�� nella ricerca del marker
	uint16_t depthTrackingTolerance;
	uint16_t updateDepthTrackingTolerance();
	Target* searchTarget();
	//parametri minimi dimensione e posizione marker valido (in pixel)
	const static int minTargetWidth = 5;
	const static int minTargetHeight = 25;
	int minTargetBottomY; //in costruttore, perch�� relativo a frame

	const static float tenPercent = 0.1;
	const static int kernelRows = 10;
	const static float kernelCols = 10;
	const static int patterFilteringConsecutiveBlacks = 5;

	uint16_t depthTrackingToleranceMin;
	//distanza dal centro per considerare fuoriuscita marker
	int* epsilonFromCenter;
	//distanza dal centro da considerare nella quale stabilizzare l'orientamento del robot rispetto al marker
	int* epsilonStabilization;
	//parametri di configurazione
	//tolleranza percentuale normalizzata (<=1) (rispetto al frame) per considerare impilati due box
	float horizontalTolerance, verticalTolerance;
	const static int obstacleROISide = 50;
	vector<Rect*>* leftROIs;
	vector<Rect*>* rightROIs;
	Rect* centerROI;

	BWPatternFiltering *bwPatternFiltering;

	uint16_t getMedianValue(vector<uint16_t> values);
	uint16_t getObjectDistance(cv::Rect objectROI, bool useOnlyDepthMap);
	/**
	 * Parametri per calcolo distanza rispetto alle dim sono innestati nella funz.
	 */
	uint16_t calculateDistanceFromDimensions(cv::Rect objectROI);
	void targetDetection(vector<Rect>& connectedComponents);

	/*si assume che markers non sia vuoto*/
	Rect getNearestTarget(vector<Rect>& markers);
	Target* selectFinalTarget(vector<Rect> roi);
	/**
	 * Tolleranza (sia orizz. che vert.) in pixel
	 */

	vector<uint16_t>* getObstaclesDistances(vector<Rect*>* ROIs);

public:
	~VisionControllerBW();
	VisionControllerBW(float horizontalTolerance, float verticalTolerance,
			int maxGhostFrames, int* epsilonFromCenter,
			int* epsilonStabilization);
	//Restituisce il Target di interesse
	Target* processFrame();
	uint16_t getFrontalDistance();
	static const uint16_t DEPTHMIN = 550;
	static const uint16_t DEPTHMAX = 4000;
	void drawLineEpsilon(int epsilon);
	vector<uint16_t>* getRightObstaclesDistances();
	vector<uint16_t>* getLeftObstaclesDistances();
};

#endif /* VISION_CONTROLLER_H_ */
